#ifndef _UBT_PROTOCOL_H_
#define _UBT_PROTOCOL_H_
#include <stdint.h>


#define UBT_SERVO_CMD_SIZE		10


/**
* @brief:
* UBT servo protocols pack and unpack
*
*                     pack
*                  --------------->
* One Line Serial                     Servo             
*                  <--------------
*                     unpack
*
* Params: id-u8, angle-u8, offset-s16, msec-u16, lckmsec-u16
* The command should be returned within 400us!
*/
class CUBTServoProtocol
{
public:
	CUBTServoProtocol();
	~CUBTServoProtocol();

	static uint8_t Checksum(uint8_t *cmd);

	// Pack
	static bool PackSetAngle(int &size, uint8_t *cmd, uint8_t id, uint8_t angle,
		uint16_t msec, uint16_t lckmsec);
	static bool PackGetAngle(int &size, uint8_t *cmd, uint8_t id);
	static bool PackSetOffset(int &size, uint8_t *cmd, uint8_t id, int16_t offset);
	static bool PackGetOffset(int &size, uint8_t *cmd, uint8_t id);
	static bool PackSetId(int &size, uint8_t *cmd, uint8_t id, uint8_t newid);
	static bool PackSetLed(int &size, uint8_t *cmd, uint8_t id, bool on);
	static bool PackTerminate(int &size, uint8_t *cmd, uint8_t id);
	static bool PackGetFirmware(int &size, uint8_t *cmd, uint8_t id);

	// Unpack
	static bool UnpackSetAngle(int size, uint8_t *cmd, uint8_t id);
	static bool UnpackGetAngle(int size, uint8_t *cmd, uint8_t id, uint8_t *angle);
	static bool UnpackSetOffset(int size, uint8_t *cmd, uint8_t id);
	static bool UnpackGetOffset(int size, uint8_t *cmd, uint8_t id, int16_t *offset);
	static bool UnpackSetId(int size, uint8_t *cmd, uint8_t id);
	static bool UnpackSetLed(int size, uint8_t *cmd, uint8_t id);
	static bool UnpackTerminate(int size, uint8_t *cmd, uint8_t id);
	static bool UnpackGetFirmware(int size, uint8_t *cmd, uint8_t id, uint32_t &firmware);
};


#endif

